#include <stdio.h>
#include <math.h>
#include <cstring>
#include <cassert>
#include <algorithm>
#include "QueueUtil.h"
#include "BasicType.h"
#include <chrono>
#include "time.h"




# define PI           3.1415927f


#define FIXED_POINT_SHIFT 16
#define FIXED_POINT_ONE (1 << FIXED_POINT_SHIFT)



void print_current_time(){
    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    printf("Current Time: %ld, %ld\n", ts.tv_sec, ts.tv_nsec);

}


// Convert floating-point to fixed-point
inline int floatToFixed(double value) {
    return static_cast<int>(value * FIXED_POINT_ONE + 0.5);
}

// Convert fixed-point to integer
inline int fixedToInt(int value) {
    return value >> FIXED_POINT_SHIFT;
}

// Fixed-point mod 65536
inline int fixedMod(int a) {
    // 等价于 a % 65536
    return a & (FIXED_POINT_ONE - 1);
}

// Fixed-point multiplication
inline int fixedMul(int a, int b) {
    return (a * b);
//    return (a * b) >> FIXED_POINT_SHIFT;
}

//inline int fixedTwoMul(int a, int b) {
//    return (a * b) >> FIXED_POINT_SHIFT;
//}

inline int fixedTwoMul(int a, int b) {
    int a_hi = a >> FIXED_POINT_SHIFT;  // 整数部分
    int a_lo = a & ((1 << FIXED_POINT_SHIFT) - 1);  // 小数部分
    int b_hi = b >> FIXED_POINT_SHIFT;
    int b_lo = b & ((1 << FIXED_POINT_SHIFT) - 1);

    // 高位部分乘法 + (交叉项 / 缩放因子) + (低位部分 / (缩放因子 * 缩放因子))
    return (a_hi * b_hi << FIXED_POINT_SHIFT) + (a_hi * b_lo + a_lo * b_hi) + ((a_lo * b_lo) >> FIXED_POINT_SHIFT);
}

inline int fixedTwoMul2(int a, int b) {
    int a_hi = a >> FIXED_POINT_SHIFT;  // a的整数部分
    int a_lo = a & ((1 << FIXED_POINT_SHIFT) - 1);  // a的小数部分
    int b_hi = b >> FIXED_POINT_SHIFT;  // b的整数部分
    int b_lo = b & ((1 << FIXED_POINT_SHIFT) - 1);  // b的小数部分

    // 高位乘法结果部分（整数部分相乘，结果保持定点位移量）
    int high_result = a_hi * b_hi;  // 直接相乘，无需再左移

    // 中位乘法结果部分（整数部分与小数部分相乘，结果无需再右移）
    int mid_result = (a_hi * b_lo + a_lo * b_hi);  // 两个交叉项

    // 低位乘法结果部分（小数部分相乘，需要右移一个定点位移量以保持结果一致）
    int low_result = (a_lo * b_lo) >> FIXED_POINT_SHIFT;  // 精度较低

    // 最终结果：将所有部分累加
    return (high_result << FIXED_POINT_SHIFT) + mid_result + low_result;
}


inline int fixedTwoMul3(int a, int b) {
    int a_hi = a >> FIXED_POINT_SHIFT;  // a的整数部分
    int a_lo = a & ((1 << FIXED_POINT_SHIFT) - 1);  // a的小数部分
    int b_hi = b >> FIXED_POINT_SHIFT;  // b的整数部分
    int b_lo = b & ((1 << FIXED_POINT_SHIFT) - 1);  // b的小数部分

    // 高位乘法结果部分 (整数部分相乘，直接得定点表示)
    int high_result = a_hi * b_hi;

    // 中位乘法结果部分 (整数部分与小数部分相乘)
    int mid_result1 = a_hi * b_lo;
    int mid_result2 = a_lo * b_hi;

    // 低位乘法结果部分 (小数部分相乘，结果右移一个定点位移量)
    int low_result = (a_lo * b_lo) >> FIXED_POINT_SHIFT;

    // 汇总结果时，避免溢出
    int result = (high_result << FIXED_POINT_SHIFT);  // 保持与定点位移量一致

    // 加入中位结果时避免溢出
    result += (mid_result1 + mid_result2);

    // 加入低位结果
    result += low_result;

    // 如果在范围外，我们可以进行一些修正
    if (result < -(1 << 31)) result = -(1 << 31);
    else if (result > (1 << 31) - 1) result = (1 << 31) - 1;

    return result;
}

inline int fixedTwoMul4(int a, int b) {
    return static_cast<int>((static_cast<long long>(a) * b) >> FIXED_POINT_SHIFT);
}

// Fixed-point division
inline int fixedDiv(int a, int b) {
    return static_cast<int>(((int64_t)a << FIXED_POINT_SHIFT) / b);
//    return (a << FIXED_POINT_SHIFT) / b;
}

Vector3* initVector3(float x, float y, float z) {
    Vector3 *vec = (Vector3*)malloc(sizeof(Vector3));
    vec->x = x;
    vec->y = y;
    vec->z = z;
    return vec;
}

Quaternion* initQuaternion(float q0, float q1, float q2, float q3) {
    Quaternion *quat = (Quaternion*)malloc(sizeof(Quaternion));
    quat->q0 = q0;
    quat->q1 = q1;
    quat->q2 = q2;
    quat->q3 = q3;
    return quat;
}

void setQuaternion(Quaternion *q, float q0, float q1, float q2, float q3) {
    q->q0 = q0;
    q->q1 = q1;
    q->q2 = q2;
    q->q3 = q3;
}

float length(Quaternion *q) {
    return sqrt((q->q0) * (q->q0) + (q->q1) * (q->q1) +
                (q->q2) * (q->q2) + (q->q3) * (q->q3));
}

void normalize(Quaternion *q) {
    float l = length(q);
    q->q0 = q->q0 / l;
    q->q1 = q->q1 / l;
    q->q2 = q->q2 / l;
    q->q3 = q->q3 / l;
}

void inverse(Quaternion *q) {
    float l2 = length(q) * length(q);
    q->q0 = q->q0 / l2;
    q->q1 = -(q->q1 / l2);
    q->q2 = -(q->q2 / l2);
    q->q3 = -(q->q3 / l2);
}

//a * b
void multiply(Quaternion *a, Quaternion *b, Quaternion *result) {
    result->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;
    result->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;
    result->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;
    result->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;
}

// rotate q by r;  thus, calculate rqr'
void rotate(Quaternion *r, Quaternion *q, Quaternion *result) {
    Quaternion rCopy;
    setQuaternion(&rCopy, r->q0, r->q1, r->q2, r->q3);
    inverse(&rCopy);
    Quaternion subResult;
    multiply(r, q, &subResult);
    multiply(&subResult, &rCopy, result);
}

void setFromAngleAxis(Quaternion *q, float angle, float vx, float vy, float vz) {
    float deg2rad = PI / 180.0;
    float deg = deg2rad * angle / 2.f;
    q->q0 = cos(deg);
    q->q1 = vx * sin(deg);
    q->q2 = vy * sin(deg);
    q->q3 = vz * sin(deg);
}

void slerp(Quaternion *q1, Quaternion *q2, float alpha, Quaternion *result) {
    float dot = q1->q0 * q2->q0 + q1->q1 * q2->q1 + q1->q2 * q2->q2 + q1->q3 * q2->q3;

    if (dot < 0.0f) {
        q2->q0 = -q2->q0;
        q2->q1 = -q2->q1;
        q2->q2 = -q2->q2;
        q2->q3 = -q2->q3;
        dot = -dot;
    }

    float scale0 = 1.0f - alpha;
    float scale1 = alpha;

    if (dot < 0.9995f) {
        float sina = sqrtf( 1.0f - dot*dot );
        float a = atan2f( sina, dot );
        scale0 = sinf((1.0f - alpha)*a)  / sina;
        scale1 = sinf(alpha*a) / sina;
    }

    result->q0 = scale0 * q1->q0 + scale1 * q2->q0;
    result->q1 = scale0 * q1->q1 + scale1 * q2->q1;
    result->q2 = scale0 * q1->q2 + scale1 * q2->q2;
    result->q3 = scale0 * q1->q3 + scale1 * q2->q3;
}

//rotation quaternion from raw motion -> smooth motion to counteract it
void calculateRotationQuaternion(Quaternion *q1, Quaternion *q2, Quaternion *result) {
    Quaternion q1Inverse;

    normalize(q1);
    normalize(q2);

    setQuaternion(&q1Inverse, q1->q0, q1->q1, q1->q2, q1->q3);
    inverse(&q1Inverse);

    result->q0 = q1Inverse.q0 * q2->q0 - q1Inverse.q1 * q2->q1 - q1Inverse.q2 * q2->q2 - q1Inverse.q3 * q2->q3;
    result->q1 = q1Inverse.q0 * q2->q1 + q1Inverse.q1 * q2->q0 + q1Inverse.q2 * q2->q3 - q1Inverse.q3 * q2->q2;
    result->q2 = q1Inverse.q0 * q2->q2 - q1Inverse.q1 * q2->q3 + q1Inverse.q2 * q2->q0 + q1Inverse.q3 * q2->q1;
    result->q3 = q1Inverse.q0 * q2->q3 + q1Inverse.q1 * q2->q2 - q1Inverse.q2 * q2->q1 + q1Inverse.q3 * q2->q0;
    normalize(result);
}


/**
 *
 * @brief Quaternion to RotationMatrix
 *
 * @param q Quaternion
 *
 */
Matrix3 toRotationMatrix(Quaternion* q)
{
    Matrix3 res;

    float tx  = 2.f * q->q1;
    float ty  = 2.f * q->q2;
    float tz  = 2.f * q->q3;
    float twx = tx * q->q0;
    float twy = ty * q->q0;
    float twz = tz * q->q0;
    float txx = tx * q->q1;
    float txy = ty * q->q1;
    float txz = tz * q->q1;
    float tyy = ty * q->q2;
    float tyz = tz * q->q2;
    float tzz = tz * q->q3;

    res.data[0][0] = 1.f - (tyy + tzz);
    res.data[0][1] = txy - twz;
    res.data[0][2] = txz + twy;
    res.data[1][0] = txy + twz;
    res.data[1][1] = 1.f - (txx + tzz);
    res.data[1][2] = tyz - twx;
    res.data[2][0] = txz - twy;
    res.data[2][1] = tyz + twx;
    res.data[2][2] = 1.f - (txx + tyy);

    return res;
}

/**
 *
 * @brief compute matrix transpose
 *
 * @param mat Matrix3
 *
 */
Matrix3 matrix_transpose(Matrix3* mat)
{
    Matrix3 mat_transpose = {0};
    for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                mat_transpose.data[j][i] = mat->data[i][j];
            }
        }
    return mat_transpose;
}


Matrix3 matrix_adjoint(Matrix3 *mat)
{
    Matrix3 adju_m = {0.0f};

    adju_m.data[0][0] =   mat->data[1][1] * mat->data[2][2] - mat->data[2][1] * mat->data[1][2];
    adju_m.data[0][1] = -(mat->data[0][1] * mat->data[2][2] - mat->data[2][1] * mat->data[0][2]);
    adju_m.data[0][2] =   mat->data[0][1] * mat->data[1][2] - mat->data[1][1] * mat->data[0][2];
    adju_m.data[1][0] = -(mat->data[1][0] * mat->data[2][2] - mat->data[2][0] * mat->data[1][2]);
    adju_m.data[1][1] =   mat->data[0][0] * mat->data[2][2] - mat->data[2][0] * mat->data[0][2];
    adju_m.data[1][2] = -(mat->data[0][0] * mat->data[1][2] - mat->data[1][0] * mat->data[0][2]);
    adju_m.data[2][0] =   mat->data[1][0] * mat->data[2][1] - mat->data[2][0] * mat->data[1][1];
    adju_m.data[2][1] = -(mat->data[0][0] * mat->data[2][1] - mat->data[2][0] * mat->data[0][1]);
    adju_m.data[2][2] =   mat->data[0][0] * mat->data[1][1] - mat->data[1][0] * mat->data[0][1];
    return adju_m;
}

Matrix3 K_inverse(Matrix3* mat)
{
    Matrix3 mat_inv = {0};
    mat_inv.data[0][0] = 1.f / mat->data[0][0];
    mat_inv.data[1][1] = 1.f / mat->data[1][1];
    mat_inv.data[0][2] = - mat->data[0][2] / mat->data[0][0];
    mat_inv.data[1][2] = - mat->data[1][2] / mat->data[1][1];
    mat_inv.data[2][2] = 1.f;

    return mat_inv;
}

/**
 *
 * @brief compute matrix inverse
 *
 * @param mat Matrix3
 *
 */
Matrix3 matrix_inverse(Matrix3* mat)
{
    float detA = mat->data[0][0] * (mat->data[1][1] * mat->data[2][2] - mat->data[1][2] * mat->data[2][1]) -
                 mat->data[0][1] * (mat->data[1][0] * mat->data[2][2] - mat->data[1][2] * mat->data[2][0]) +
                 mat->data[0][2] * (mat->data[1][0] * mat->data[2][1] - mat->data[1][1] * mat->data[2][0]);

    if(fabs(detA) < 1e-5)
    {
        printf("mat can not inverse\n");
        return *mat;
    }


    Matrix3 mat_adj = matrix_adjoint(mat);

    Matrix3 mat_inv = {0};
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            mat_inv.data[i][j] = mat_adj.data[i][j] / detA;
        }
    }
    return mat_inv;
}

void matrix_multiply(Matrix3* result, const Matrix3* a, const Matrix3* b) {
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            result->data[i][j] = 0;
            for (int k = 0; k < 3; ++k) {
                result->data[i][j] += a->data[i][k] * b->data[k][j];
            }
        }
    }
}


void initUnitCircle(Matrix3 *matrix) {
    matrix->data[0][0] = 1.0f;
    matrix->data[0][1] = 0.0f;
    matrix->data[0][2] = 0.0f;
    matrix->data[1][0] = 0.0f;
    matrix->data[1][1] = 1.0f;
    matrix->data[1][2] = 0.0f;
    matrix->data[2][0] = 0.0f;
    matrix->data[2][1] = 0.0f;
    matrix->data[2][2] = 1.0f;
}

// // hard code 内参
// void initK(Matrix3 *matrix) {
//     matrix->data[0][0] = 942.9f;
//     matrix->data[0][1] = 0.0f;
//     matrix->data[0][2] = 438.7f;
//     matrix->data[1][0] = 0.0f;
//     matrix->data[1][1] = 943.6f;
//     matrix->data[1][2] = 290.9f;
//     matrix->data[2][0] = 0.0f;
//     matrix->data[2][1] = 0.0f;
//     matrix->data[2][2] = 1.0f;
// }

// void initC2I(Matrix3 *matrix) {
//     matrix->data[0][0] = 0.9f;
//     matrix->data[0][1] = 0.42f;
//     matrix->data[0][2] = 0.112f;
//     matrix->data[1][0] = -0.156f;
//     matrix->data[1][1] = 0.072f;
//     matrix->data[1][2] = 0.985f;
//     matrix->data[2][0] = 0.406f;
//     matrix->data[2][1] = -0.904f;
//     matrix->data[2][2] = 0.131f;
// }

// hard code 内参
void initK(Matrix3 *matrix) {
    matrix->data[0][0] = 458.654f;
    matrix->data[0][1] = 0.0f;
    matrix->data[0][2] = 367.215f;
    matrix->data[1][0] = 0.0f;
    matrix->data[1][1] = 457.296f;
    matrix->data[1][2] = 248.375f;
    matrix->data[2][0] = 0.0f;
    matrix->data[2][1] = 0.0f;
    matrix->data[2][2] = 1.0f;
}


// gt
void initC2I_gt(Matrix3 *matrix) {
    matrix->data[0][0] = 0.0148655429818f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.00414029679422f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.0149672133247f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.0257744366974f;
    matrix->data[2][1] = 0.00375618835797f;
    matrix->data[2][2] = 0.999660727178f;
}


// x n 1.5
void initC2I5(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.999646675811f;
    matrix->data[0][2] = -0.022034953393f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.014288930337f;
    matrix->data[1][2] = 0.026098513848f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = -0.022413165975f;
    matrix->data[2][2] = 0.999416493962f;
}

// y -1.5
void initC2I6(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014968829267f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.003749743468f;
    matrix->data[1][0] = 0.999887879802f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = -0.000458640560f;
    matrix->data[2][0] = 0.000402462739f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.999992864511f;
}

// z -1.5
void initC2I7(Matrix3 *matrix) {
    matrix->data[0][0] = 0.041034280342f;
    matrix->data[0][1] = -0.999149160926f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.998822929734f;
    matrix->data[1][1] = 0.041127442873f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025863929990f;
    matrix->data[2][1] = 0.003080205109f;
    matrix->data[2][2] = 0.999660727178f;
}

// d -1.5
void initC2I8(Matrix3 *matrix) {
    matrix->data[0][0] = 0.029922450493f;
    matrix->data[0][1] = -0.999488242420f;
    matrix->data[0][2] = -0.011309297995f;
    matrix->data[1][0] = 0.999495989316f;
    matrix->data[1][1] = 0.029798836894f;
    matrix->data[1][2] = 0.010945166071f;
    matrix->data[2][0] = -0.010602560872f;
    matrix->data[2][1] = -0.011631104178f;
    matrix->data[2][2] = 0.999876143889f;
}

// x -1
void initC2I9(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.999800901132f;
    matrix->data[0][2] = -0.013310662167f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.014516135865f;
    matrix->data[1][2] = 0.025972827238f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = -0.013690869038f;
    matrix->data[2][2] = 0.999574028533f;
}

// y -1
void initC2I10(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014935537030f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.003880226709f;
    matrix->data[1][0] = 0.999853809478f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = 0.008266933981f;
    matrix->data[2][0] = -0.008324025816f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.999958299952f;
}


// z -1
void initC2I11(Matrix3 *matrix) {
    matrix->data[0][0] = 0.032313607262f;
    matrix->data[0][1] = -0.999469203492f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.999143797708f;
    matrix->data[1][1] = 0.032409613113f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025836065652f;
    matrix->data[2][1] = 0.003305790327f;
    matrix->data[2][2] = 0.999660727178f;
}



// d -1
void initC2I12(Matrix3 *matrix) {
    matrix->data[0][0] = 0.024930151588f;
    matrix->data[0][1] = -0.999670371126f;
    matrix->data[0][2] = -0.006134870384f;
    matrix->data[1][0] = 0.999566124461f;
    matrix->data[1][1] = 0.024830321694f;
    matrix->data[1][2] = 0.015843546126f;
    matrix->data[2][0] = -0.015685992831f;
    matrix->data[2][1] = -0.006527190620f;
    matrix->data[2][2] = 0.999855662289f;
}


// x -0.5
void initC2I13(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.999878987743f;
    matrix->data[0][2] = -0.004585357283f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.014742235934f;
    matrix->data[1][2] = 0.025845162698f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = -0.004967529488f;
    matrix->data[2][2] = 0.999655441672f;
}


//y -0.5
void initC2I14(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014901107395f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.004010414456f;
    matrix->data[1][0] = 0.999743596416f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = 0.016991878963f;
    matrix->data[2][0] = -0.017049880464f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.999847584697f;
}

// z -0.5
void initC2I15(Matrix3 *matrix) {
    matrix->data[0][0] = 0.023590473375f;
    matrix->data[0][1] = -0.999713132608f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.999388577013f;
    matrix->data[1][1] = 0.023689315235f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025806233797f;
    matrix->data[2][1] = 0.003531123797f;
    matrix->data[2][2] = 0.999660727178f;
}

// d -0.5
void initC2I16(Matrix3 *matrix) {
    matrix->data[0][0] = 0.019911055009f;
    matrix->data[0][1] = -0.999801270208f;
    matrix->data[0][2] = -0.000984874723f;
    matrix->data[1][0] = 0.999586544877f;
    matrix->data[1][1] = 0.019886321669f;
    matrix->data[1][2] = 0.020767125735f;
    matrix->data[2][0] = -0.020743413153f;
    matrix->data[2][1] = -0.001397962905f;
    matrix->data[2][2] = 0.999783854896f;
}

// x 0.5
void initC2I17(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.999806726849f;
    matrix->data[0][2] = 0.012865635572f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.015191050904f;
    matrix->data[1][2] = 0.025583938861f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = 0.012479620156f;
    matrix->data[2][2] = 0.999589884650f;
}


// y 0.5
void initC2I18(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014828846501f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.004269863833f;
    matrix->data[1][0] = 0.999294781446f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = 0.034437222596f;
    matrix->data[2][0] = -0.034497030108f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.999397741624f;
}

//z 0.5
void initC2I19(Matrix3 *matrix) {
    matrix->data[0][0] = 0.006139480521f;
    matrix->data[0][1] = -0.999972581985f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.999649800849f;
    matrix->data[1][1] = 0.006243971603f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025740676775f;
    matrix->data[2][1] = 0.003980966871f;
    matrix->data[2][2] = 0.999660727178f;
}
// d 0.5
void initC2I20(Matrix3 *matrix) {
    matrix->data[0][0] = 0.009793999741f;
    matrix->data[0][1] = -0.999909343529f;
    matrix->data[0][2] = 0.009240253867f;
    matrix->data[1][0] = 0.999478239086f;
    matrix->data[1][1] = 0.010073371271f;
    matrix->data[1][2] = 0.030688381925f;
    matrix->data[2][0] = -0.030778680332f;
    matrix->data[2][1] = 0.008934870659f;
    matrix->data[2][2] = 0.999486288512f;
}
// x 1
void initC2I21(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.999656384847f;
    matrix->data[0][2] = 0.021589994581f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.015413731626f;
    matrix->data[1][2] = 0.025450399458f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = 0.021202101582f;
    matrix->data[2][2] = 0.999442919481f;
}

// y 1
void initC2I22(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014791020745f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.004399105705f;
    matrix->data[1][0] = 0.998956213717f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = 0.043156292715f;
    matrix->data[2][0] = -0.043216996436f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.999058648062f;
}

// z 1
void initC2I23(Matrix3 *matrix) {
    matrix->data[0][0] = -0.002587049486f;
    matrix->data[0][1] = -0.999988082488f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.999666225488f;
    matrix->data[1][1] = -0.002479745621f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025704956600f;
    matrix->data[2][1] = 0.004205442217f;
    matrix->data[2][2] = 0.999660727178f;
}


// d 1
void initC2I24(Matrix3 *matrix) {
    matrix->data[0][0] = 0.004696811503f;
    matrix->data[0][1] = -0.999886509538f;
    matrix->data[0][2] = 0.014314608113f;
    matrix->data[1][0] = 0.999349521127f;
    matrix->data[1][1] = 0.005205168191f;
    matrix->data[1][2] = 0.035685302963f;
    matrix->data[2][0] = -0.035755762965f;
    matrix->data[2][1] = 0.014137689621f;
    matrix->data[2][2] = 0.999260552182f;
}

// x 1.5
void initC2I(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.999429915141f;
    matrix->data[0][2] = 0.030312709429f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.015635238533f;
    matrix->data[1][2] = 0.025314921909f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = 0.029922968387f;
    matrix->data[2][2] = 0.999219842865f;
}

// y 1.5
void initC2I43(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014752068597f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.004528012568f;
    matrix->data[1][0] = 0.998541571605f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = 0.051872076316f;
    matrix->data[2][0] = -0.051933671622f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.998643472317f;
}

// z 1.5
void initC2I42(Matrix3 *matrix) {
    matrix->data[0][0] = -0.011313382479f;
    matrix->data[0][1] = -0.999927430026f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.999606521673f;
    matrix->data[1][1] = -0.011203274003f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025667278893f;
    matrix->data[2][1] = 0.004429597303f;
    matrix->data[2][2] = 0.999660727178f;
}

//d1.5
void initC2I41(Matrix3 *matrix) {
    matrix->data[0][0] = -0.000425633560f;
    matrix->data[0][1] = -0.999812429463f;
    matrix->data[0][2] = 0.019362973101f;
    matrix->data[1][0] = 0.999171104934f;
    matrix->data[1][1] = 0.000362974818f;
    matrix->data[1][2] = 0.040705912529f;
    matrix->data[2][0] = -0.040705305571f;
    matrix->data[2][1] = 0.019364249030f;
    matrix->data[2][2] = 0.998983535379f;
}





//x 2
void initC2I37(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.999127334977f;
    matrix->data[0][2] = 0.039033115846f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.015855554756f;
    matrix->data[1][2] = 0.025177516530f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = 0.038641556441f;
    matrix->data[2][2] = 0.998920671790f;
}

// y 2
void initC2I38(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014711993021f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.004656574606f;
    matrix->data[1][0] = 0.998050886687f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = 0.060583909658f;
    matrix->data[2][0] = -0.060646391859f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.998152246004f;
}
// z 2
void initC2I39(Matrix3 *matrix) {
    matrix->data[0][0] = -0.020038853914f;
    matrix->data[0][1] = -0.999790629220f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.999470693951f;
    matrix->data[1][1] = -0.019925949213f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025627646524f;
    matrix->data[2][1] = 0.004653415058f;
    matrix->data[2][2] = 0.999660727178f;
}


// d 2
void initC2I40(Matrix3 *matrix) {
    matrix->data[0][0] = -0.005572945354f;
    matrix->data[0][1] = -0.999687108946f;
    matrix->data[0][2] = 0.024384964378f;
    matrix->data[1][0] = 0.998943004093f;
    matrix->data[1][1] = -0.004452840095f;
    matrix->data[1][2] = 0.045749828283f;
    matrix->data[2][0] = -0.045626931223f;
    matrix->data[2][1] = 0.024614150864f;
    matrix->data[2][2] = 0.998655259198f;
}



// x 2.5
void initC2I25(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.998748667399f;
    matrix->data[0][2] = 0.047750549741f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.016074663516f;
    matrix->data[1][2] = 0.025038193786f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = 0.047357201792f;
    matrix->data[2][2] = 0.998545429037f;
}

// y 2.5
void initC2I26(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014670797069f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.004784782027f;
    matrix->data[1][0] = 0.997484196329f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = 0.069291129300f;
    matrix->data[2][0] = -0.069354493638f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.997585006534f;
}

// z 2.5
void initC2I27(Matrix3 *matrix) {
    matrix->data[0][0] = -0.028762799313f;
    matrix->data[0][1] = -0.999577690486f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.999258752667f;
    matrix->data[1][1] = -0.028647106984f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025586062510f;
    matrix->data[2][1] = 0.004876878437f;
    matrix->data[2][2] = 0.999660727178f;
}
// d 2.5
void initC2I28(Matrix3 *matrix) {
    matrix->data[0][0] = -0.010744731893f;
    matrix->data[0][1] = -0.999510557530f;
    matrix->data[0][2] = 0.029380199501f;
    matrix->data[1][0] = 0.998665235977f;
    matrix->data[1][1] = -0.009241909806f;
    matrix->data[1][2] = 0.050816666111f;
    matrix->data[2][0] = -0.050520265122f;
    matrix->data[2][1] = 0.029886995321f;
    matrix->data[2][2] = 0.998275748640f;
}

// x 3
void initC2I29(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.998293941243f;
    matrix->data[0][2] = 0.056464347246f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.016292548129f;
    matrix->data[1][2] = 0.024896964286f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = 0.056069240708f;
    matrix->data[2][2] = 0.998094143185f;
}

// y 3
void initC2I30(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014628483880f;
    matrix->data[0][1] = -0.999880929698f;
    matrix->data[0][2] = 0.004912625068f;
    matrix->data[1][0] = 0.996841543687f;
    matrix->data[1][1] = 0.014967213325f;
    matrix->data[1][2] = 0.077993072154f;
    matrix->data[2][0] = -0.078057313803f;
    matrix->data[2][1] = 0.003756188358f;
    matrix->data[2][2] = 0.996941797103f;
}

// z 3
void initC2I31(Matrix3 *matrix) {
    matrix->data[0][0] = -0.037484554313f;
    matrix->data[0][1] = -0.999288630042f;
    matrix->data[0][2] = 0.004140296794f;
    matrix->data[1][0] = 0.998970713959f;
    matrix->data[1][1] = -0.037366083167f;
    matrix->data[1][2] = 0.025715529948f;
    matrix->data[2][0] = -0.025542530018f;
    matrix->data[2][1] = 0.005099970424f;
    matrix->data[2][2] = 0.999660727178f;
}


// d 3
void initC2I32(Matrix3 *matrix) {
    matrix->data[0][0] = -0.015940599324f;
    matrix->data[0][1] = -0.999282788661f;
    matrix->data[0][2] = 0.034348298063f;
    matrix->data[1][0] = 0.998337821737f;
    matrix->data[1][1] = -0.014003869610f;
    matrix->data[1][2] = 0.055906040154f;
    matrix->data[2][0] = -0.055384934621f;
    matrix->data[2][1] = 0.035182380854f;
    matrix->data[2][2] = 0.997845032605f;
}

// x 3.5
void initC2I33(Matrix3 *matrix) {
    matrix->data[0][0] = 0.014865542982f;
    matrix->data[0][1] = -0.997763191138f;
    matrix->data[0][2] = 0.065173844773f;
    matrix->data[1][0] = 0.999557249008f;
    matrix->data[1][1] = 0.016509192002f;
    matrix->data[1][2] = 0.024753838787f;
    matrix->data[2][0] = -0.025774436697f;
    matrix->data[2][1] = 0.064777009734f;
    matrix->data[2][2] = 0.997566848599f;
}







void invert(const MatrixCD* src, MatrixCD* dst) {
    double det = src->data[0] * (src->data[4] * src->data[8] - src->data[7] * src->data[5]) -
                 src->data[1] * (src->data[3] * src->data[8] - src->data[5] * src->data[6]) +
                 src->data[2] * (src->data[3] * src->data[7] - src->data[4] * src->data[6]);

    assert(det != 0); // Non-singular matrix

    double invDet = 1.0 / det;

    dst->data[0] = (src->data[4] * src->data[8] - src->data[7] * src->data[5]) * invDet;
    dst->data[1] = (src->data[2] * src->data[7] - src->data[1] * src->data[8]) * invDet;
    dst->data[2] = (src->data[1] * src->data[5] - src->data[2] * src->data[4]) * invDet;
    dst->data[3] = (src->data[5] * src->data[6] - src->data[3] * src->data[8]) * invDet;
    dst->data[4] = (src->data[0] * src->data[8] - src->data[2] * src->data[6]) * invDet;
    dst->data[5] = (src->data[3] * src->data[2] - src->data[0] * src->data[5]) * invDet;
    dst->data[6] = (src->data[3] * src->data[7] - src->data[4] * src->data[6]) * invDet;
    dst->data[7] = (src->data[6] * src->data[1] - src->data[0] * src->data[7]) * invDet;
    dst->data[8] = (src->data[0] * src->data[4] - src->data[3] * src->data[1]) * invDet;
}


inline int warp_min(int a, int b) {
    return (a < b) ? a : b;
}

inline int warp_max(int a, int b) {
    return (a > b) ? a : b;
}


int warpPerspectiveFixedPoint(const ImageCD* src, ImageCD* dst, const MatrixCD* M, int flags, int borderMode, const ScalarCD* borderValue) {
    assert(src->data != NULL && dst->data != NULL && M->data != NULL);
    assert(src->width > 0 && src->height > 0 && dst->width > 0 && dst->height > 0);
    assert(src->data != dst->data);
    assert(M->rows == 3 && M->cols == 3);

    double matM[9];
    MatrixCD matM_struct = {3, 3, matM};

    if (!(flags & CD_WARP_INVERSE_MAP))
        invert(M, &matM_struct);
    else
        for (int i = 0; i < 9; ++i)
            matM[i] = M->data[i];

    // Convert matrix elements to fixed-point
    int M0 = floatToFixed(matM[0]), M1 = floatToFixed(matM[1]), M2 = floatToFixed(matM[2]);
    int M3 = floatToFixed(matM[3]), M4 = floatToFixed(matM[4]), M5 = floatToFixed(matM[5]);
    int M6 = floatToFixed(matM[6]), M7 = floatToFixed(matM[7]), M8 = floatToFixed(matM[8]);

//    printf("float M: m0-%f-m1-%f-m2-%f-m3-%f-m4-%f-m5-%f-m6-%f-m7-%f-m8-%f\n",
//           matM[0], matM[1], matM[2], matM[3], matM[4], matM[5], matM[6], matM[7], matM[8]);
//
//    printf("fixed M: m0-%d-m1-%d-m2-%d-m3-%d-m4-%d-m5-%d-m6-%d-m7-%d-m8-%d\n",
//           M0, M1, M2, M3, M4, M5, M6, M7, M8);

    int x, y, x1, y1, width = dst->width, height = dst->height;

    for (y = 0; y < height; y++) {
        for (x = 0; x < width; x++) {
//            for (y = 0; y < 3; y++) {
//                for (x = 0; x < 3; x++) {
            // Compute the source pixel coordinates using fixed-point arithmetic
            int X = fixedMul(M0, x) + fixedMul(M1, y) + M2;
            int Y = fixedMul(M3, x) + fixedMul(M4, y) + M5;
            int W = fixedMul(M6, x) + fixedMul(M7, y) + M8;

            if (W == 0) {
                dst->data[y * dst->width + x] = (unsigned char)borderValue->val[0];
                continue;
            }
//            printf("1. x-y-X-Y-W %d %d %d %d %d \n", x, y, X, Y, W);

            int invW = fixedDiv(FIXED_POINT_ONE, W);
//            printf("2.1. x-y-X-Y-W-invW %d\t%d\t%d\t%d\t%d\t%d \n", x, y, X, Y, W, invW);
            X = fixedTwoMul(X, invW);
            Y = fixedTwoMul(Y, invW);
//            printf("2.2. x-y-X-Y-W-invW %d\t%d\t%d\t%d\t%d\t%d \n", x, y, X, Y, W, invW);

//          取整
            x1 = fixedToInt(X);
            y1 = fixedToInt(Y);
//            printf("3. x-y-x1-y1- X- Y: %d, %d, %d, %d, %d, %d \n", x, y, x1, y1, X, Y);

            if (flags == CD_INTER_NEAREST) {
                if (x1 >= 0 && x1 < src->width && y1 >= 0 && y1 < src->height) {
                    dst->data[y * dst->width + x] = src->data[y1 * src->width + x1];
                } else {
                    dst->data[y * dst->width + x] = (unsigned char)borderValue->val[0];
                }
            } else {
                // Bilinear interpolation
                int X1 = warp_min(warp_max(x1, 0), src->width - 1);
                int Y1 = warp_min(warp_max(y1, 0), src->height - 1);
                int X2 = warp_min(X1 + 1, src->width - 1);
                int Y2 = warp_min(Y1 + 1, src->height - 1);

//                int dx = X - floatToFixed(X1);
//                int dy = Y - floatToFixed(Y1);

                int dx = fixedMod(X);
                int dy = fixedMod(Y);
//                printf("4. x-y-X1-Y1-dx-dy: %d, %d, %d, %d, %d, %d, \n",  x, y, X1, Y1, dx, dy);

                unsigned char p1 = src->data[Y1 * src->width + X1];
                unsigned char p2 = src->data[Y1 * src->width + X2];
                unsigned char p3 = src->data[Y2 * src->width + X1];
                unsigned char p4 = src->data[Y2 * src->width + X2];

//                int a1, a2, a3, a4;
//                a1 = fixedTwoMul4((FIXED_POINT_ONE - dx), FIXED_POINT_ONE - dy);
//                a2 = fixedTwoMul4(dx, FIXED_POINT_ONE - dy);
//                a3 = fixedTwoMul4((FIXED_POINT_ONE - dx), dy);
//                a4 = fixedTwoMul4(dx, dy);

                int interpolated = fixedTwoMul4((FIXED_POINT_ONE - dx), FIXED_POINT_ONE - dy) * p1 +
                                   fixedTwoMul4(dx, FIXED_POINT_ONE - dy) * p2 +
                                   fixedTwoMul4((FIXED_POINT_ONE - dx), dy) * p3 +
                                   fixedTwoMul4(dx, dy) * p4;

//                printf("5. x-y-X1-Y1-dx-dy: %d, %d, %d, %d, %d, %d, \n",  x, y, X1, Y1, dx, dy);
//                printf("6. a1, a2, a3, a4, interpolated: %d, %d, %d, %d, %d\n",  a1, a2, a3, a4, interpolated);

                dst->data[y * dst->width + x] = (unsigned char)(interpolated >> FIXED_POINT_SHIFT);
            }
        }
    }

    return 0;
}

int warpPerspectiveCD(const ImageCD* src, ImageCD* dst, const MatrixCD* M, int flags,int borderMode, const ScalarCD* borderValue) {
    assert(src->data != NULL && dst->data != NULL && M->data != NULL);
    assert(src->width > 0 && src->height > 0 && dst->width > 0 && dst->height > 0);
    assert(src->data != dst->data);
    assert(M->rows == 3 && M->cols == 3);

    double matM[9];
    MatrixCD matM_struct = {3, 3, matM};

    if (!(flags & CD_WARP_INVERSE_MAP))
        invert(M, &matM_struct);
    else
        for (int i = 0; i < 9; ++i)
            matM[i] = M->data[i];

    int x, y, x1, y1, width = dst->width, height = dst->height, srcWidth = src->width, srcHeight = src->height;

//    printf("nofix: float M: m0-%f-m1-%f-m2-%f-m3-%f-m4-%f-m5-%f-m6-%f-m7-%f-m8-%f\n",
//           matM[0], matM[1], matM[2], matM[3], matM[4], matM[5], matM[6], matM[7], matM[8]);

    for (y = 0; y < height; y++) {
        for (x = 0; x < width; x++) {
            double X = matM[0] * x + matM[1] * y + matM[2];
            double Y = matM[3] * x + matM[4] * y + matM[5];
            double W = matM[6] * x + matM[7] * y + matM[8];
            W = W ? 1.0 / W : 0;
            X *= W;
            Y *= W;

            x1 = (int)X;
            y1 = (int)Y;

            if (flags == CD_INTER_NEAREST) {
                if (x1 >= 0 && x1 < src->width && y1 >= 0 && y1 < src->height) {
                    dst->data[y * dst->width + x] = src->data[y1 * src->width + x1];
                } else {
                    dst->data[y * dst->width + x] = (unsigned char)borderValue->val[0];
                }
            } else {
                int X1 = warp_min(warp_max(x1, 0), src->width - 1);
                int Y1 = warp_min(warp_max(y1, 0), src->height - 1);
                int X2 = warp_min(X1 + 1, src->width - 1);
                int Y2 = warp_min(Y1 + 1, src->height - 1);

                // X:矩阵计算后的小数值 X1:X的取整值
                double dx = X - X1;
                double dy = Y - Y1;

                unsigned char p1 = src->data[Y1 * src->width + X1];
                unsigned char p2 = src->data[Y1 * src->width + X2];
                unsigned char p3 = src->data[Y2 * src->width + X1];
                unsigned char p4 = src->data[Y2 * src->width + X2];

                dst->data[y * dst->width + x] = (unsigned char)(
                        (1 - dx) * (1 - dy) * p1 +
                        dx * (1 - dy) * p2 +
                        (1 - dx) * dy * p3 +
                        dx * dy * p4
                );
            }
        }
    }
    return 0;
}

int warpPerspectiveQuarter(const unsigned char* src, unsigned char* dst,
                       const unsigned char* srcU, unsigned char* dstU,
                       const unsigned char* srcV, unsigned char* dstV,
                       int start, int end, int width, int height,
                       const MatrixCD* M, int flags,int borderMode, const ScalarCD* borderValue) {

    print_current_time();
    double matM[9];
    MatrixCD matM_struct = {3, 3, matM};

    if (!(flags & CD_WARP_INVERSE_MAP))
        invert(M, &matM_struct);
    else
        for (int i = 0; i < 9; ++i)
            matM[i] = M->data[i];

    int x, y, x1, y1;

    printf("start %d, end %d\n", start, end);

    for (y = start; y < end; y++) {
        for (x = 0; x < width; x++) {

            double X = matM[0] * x + matM[1] * y + matM[2];
            double Y = matM[3] * x + matM[4] * y + matM[5];
            double W = matM[6] * x + matM[7] * y + matM[8];
            W = W ? 1.0 / W : 0;
            X *= W;
            Y *= W;

            x1 = (int)X;
            y1 = (int)Y;

            if (flags == CD_INTER_NEAREST) {
                printf("neibor here\n");
//                if (x1 >= 0 && x1 < src->width && y1 >= 0 && y1 < src->height) {
//                    dst->data[y * dst->width + x] = src->data[y1 * src->width + x1];
//                } else {
//                    dst->data[y * dst->width + x] = (unsigned char)borderValue->val[0];
//                }
            } else {
                int X1 = warp_min(warp_max(x1, 0), width - 1);
                int Y1 = warp_min(warp_max(y1, 0), height - 1);
                int X2 = warp_min(X1 + 1, width - 1);
                int Y2 = warp_min(Y1 + 1, height - 1);

                // X:矩阵计算后的小数值 X1:X的取整值
                double dx = X - X1;
                double dy = Y - Y1;

                int n1, n2, n3, n4;
                n1 = Y1 * width + X1;
                n2 = Y1 * width + X2;
                n3 = Y2 * width + X1;
                n4 = Y2 * width + X2;

                unsigned char p1 = src[n1];
                unsigned char p2 = src[n2];
                unsigned char p3 = src[n3];
                unsigned char p4 = src[n4];


                dst[y * width + x] = (unsigned char)(
                        (1 - dx) * (1 - dy) * p1 +
                        dx * (1 - dy) * p2 +
                        (1 - dx) * dy * p3 +
                        dx * dy * p4
                );

                // dstU[(y/2) * (width/2) + (x % width) / 2]
                //         = srcU[(Y1/2) * (width/2) + (X1 % width) / 2];

                // dstV[(y/2) * (width/2) + (x % width) / 2]
                //         = srcV[(Y1/2) * (width/2) + (X1 % width) / 2];

            }
        }
    }
    print_current_time();
    return 0;
}


int warpPerspectiveUV(const ImageCD* src, ImageCD* dst,
                      const ImageCD* srcU, ImageCD* dstU,
                      const ImageCD* srcV, ImageCD* dstV,
                      const MatrixCD* M, int flags, int borderMode, const ScalarCD* borderValue) {

    assert(src->data != NULL && dst->data != NULL && M->data != NULL);
    assert(src->width > 0 && src->height > 0 && dst->width > 0 && dst->height > 0);
    assert(src->data != dst->data);
    assert(M->rows == 3 && M->cols == 3);

    double matM[9];
    MatrixCD matM_struct = {3, 3, matM};

    if (!(flags & CD_WARP_INVERSE_MAP))
        invert(M, &matM_struct);
    else
        for (int i = 0; i < 9; ++i)
            matM[i] = M->data[i];

    int x, y, x1, y1, width = dst->width, height = dst->height, srcWidth = src->width, srcHeight = src->height;

    for (y = 0; y < height; y++) {
        for (x = 0; x < width; x++) {
            double X = matM[0] * x + matM[1] * y + matM[2];
            double Y = matM[3] * x + matM[4] * y + matM[5];
            double W = matM[6] * x + matM[7] * y + matM[8];
            W = W ? 1.0 / W : 0;
            X *= W;
            Y *= W;

            x1 = (int)X;
            y1 = (int)Y;

            if (flags == CD_INTER_NEAREST) {
                if (x1 >= 0 && x1 < src->width && y1 >= 0 && y1 < src->height) {
                    dst->data[y * dst->width + x] = src->data[y1 * src->width + x1];
                } else {
                    dst->data[y * dst->width + x] = (unsigned char)borderValue->val[0];
                }
            } else {
                int X1 = warp_min(warp_max(x1, 0), src->width - 1);
                int Y1 = warp_min(warp_max(y1, 0), src->height - 1);
                int X2 = warp_min(X1 + 1, src->width - 1);
                int Y2 = warp_min(Y1 + 1, src->height - 1);

                // X:矩阵计算后的小数值 X1:X的取整值
                double dx = X - X1;
                double dy = Y - Y1;

                int n1, n2, n3, n4;
                n1 = Y1 * src->width + X1;
                n2 = Y1 * src->width + X2;
                n3 = Y2 * src->width + X1;
                n4 = Y2 * src->width + X2;

                unsigned char p1 = src->data[n1];
                unsigned char p2 = src->data[n2];
                unsigned char p3 = src->data[n3];
                unsigned char p4 = src->data[n4];

                dst->data[y * dst->width + x] = (unsigned char)(
                        (1 - dx) * (1 - dy) * p1 +
                        dx * (1 - dy) * p2 +
                        (1 - dx) * dy * p3 +
                        dx * dy * p4
                );

                dstU->data[(y/2) * (dst->width/2) + (x % dst->width) / 2]
                    = srcU->data[(Y1/2) * (dst->width/2) + (X1 % dst->width) / 2];

                dstV->data[(y/2) * (dst->width/2) + (x % dst->width) / 2]
                        = srcV->data[(Y1/2) * (dst->width/2) + (X1 % dst->width) / 2];

            }
        }
    }
    return 0;
}

//int test_main() {
//    // 读取图像
//    cv::Mat image = cv::imread("imagePath", cv::IMREAD_COLOR);
//    if (image.empty()) {
//        std::cerr << "无法读取图像" << std::endl;
//        return -1;
//    }
//
//    // 将图像从BGR转换为YUV420p
//    cv::Mat yuv_image;
//    cv::cvtColor(image, yuv_image, cv::COLOR_BGR2YUV_I420);
//
//    // 获取YUV通道的指针
//    int width = image.cols;
//    int height = image.rows;
//    unsigned char* y_plane = yuv_image.data;
//    unsigned char* u_plane = yuv_image.data + width * height;
//    unsigned char* v_plane = u_plane + (width * height) / 4;
//
//    // 创建目标图像
//    Image srcY = {width, height, y_plane};
//    Image dstY = {width, height, (unsigned char*)malloc(width * height)};
//    Image srcU = {width / 2, height / 2, u_plane};
//    Image dstU = {width / 2, height / 2, (unsigned char*)malloc((width * height) / 4)};
//    Image srcV = {width / 2, height / 2, v_plane};
//    Image dstV = {width / 2, height / 2, (unsigned char*)malloc((width * height) / 4)};
//
//    // 计算旋转30度的透视变换矩阵
//    double angle = 30.0 * CV_PI / 180.0;
//    double alpha = cos(angle);
//    double beta = sin(angle);
//    double M_data[9] = {
//            alpha, beta, (1 - alpha) * (width / 2) - beta * (height / 2),
//            -beta, alpha, beta * (width / 2) + (1 - alpha) * (height / 2),
//            0, 0, 1
//    };
//    Matrix M = {3, 3, M_data};
//
//    // 定义边界值
//    Scalar borderValue = {{0, 0, 0, 0}};
//
//    // 对Y通道应用透视变换
//    warpPerspective(&srcY, &dstY, &M, INTER_LINEAR, BORDER_CONSTANT, &borderValue);
//
//    // 对U通道应用透视变换
//    warpPerspective(&srcU, &dstU, &M, INTER_LINEAR, BORDER_CONSTANT, &borderValue);
//
//    // 对V通道应用透视变换
//    warpPerspective(&srcV, &dstV, &M, INTER_LINEAR, BORDER_CONSTANT, &borderValue);
//
//    // 将结果保存到文件（可选）
//    cv::Mat dst_yuv_image(height + height / 2, width, CV_8UC1);
//    memcpy(dst_yuv_image.data, dstY.data, width * height);
//    memcpy(dst_yuv_image.data + width * height, dstU.data, (width * height) / 4);
//    memcpy(dst_yuv_image.data + width * height + (width * height) / 4, dstV.data, (width * height) / 4);
//    cv::Mat dst_bgr_image;
//    cv::cvtColor(dst_yuv_image, dst_bgr_image, cv::COLOR_YUV2BGR_I420);
//    cv::imwrite("output.jpg", dst_bgr_image);
//
//    // 释放内存
//    free(dstY.data);
//    free(dstU.data);
//    free(dstV.data);
//
//    return 0;
//}

